/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : attitude_ctrl.hpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月24日
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __ATTITUDE_CTRL_HPP
#define __ATTITUDE_CTRL_HPP

#include "userlib/userlib.hpp"
#include "system/system.hpp"
#include "control/pid/pid.hpp"

#ifdef __cplusplus
extern "C" {
#endif

#define QUADX		//X字型四轴
//#define OCTAP		//十字型八轴
#define PWM_IDLE	80


#ifdef __cplusplus
}
#endif

#ifdef __cplusplus

using namespace std;

class ATTITUDE_CTRL
{
public:
	ATTITUDE_CTRL();
	~ATTITUDE_CTRL();
	void motor_Output();

	void Attitude_Ctrl_Init();

	void Attitude_ctrl_run();

	void pid_Para_Update();

private:
	float HoldYaw;		//锁尾偏航角度
	float SetAng[3];	//角度环目标值
	float SetDAng[3];	//角速度环目标值
	float FeedBackDAng[3];
	float FeedBackAng[3];

	float ROL_OUT;
	float PIT_OUT;
	float YAW_OUT;
	float Z_OUT;
	float FOREWARD;
	float THROTTLE;

	u8 pid_Para_Update_Time;


	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;
	ahrs_euler_msg ahrsEuler;
	Motor_PWM_msg motorPWM;
	Pid_msg pid_para;
	sensor_gyro_filter_msg gyro_filter;
	Inloop_control_msg inloop_control;
	mahonyAhrs_msg mahonyAhrs_Ang;
	RC_command_msg rcCommand;
	height_msg height;

};


#endif



#endif


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
